/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#ifndef RMNKSOLVER_H
#define RMNKSOLVER_H

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/smart_ptr.hpp>

#include <deque>

#include "Polynomial.h"

// Przestrzeń nazw
namespace RafMat
{
namespace SimEngine
{

class RmnkSolver;

typedef boost::shared_ptr<RmnkSolver> RmnkSolverSPtr;

/** Klasa do identyfikacji */
class RmnkSolver
{
public:
    /** Mnożnik macierzy kowarancji w chwili zerowej (powinna być duża wartość). */
    static const double INITIAL_COVARIANCE_FACTOR;

    /** Mnożnik do oceny szumu. */
    static const double OMEGA_FACTOR;

    typedef boost::numeric::ublas::matrix<double> Matrix;
    typedef boost::numeric::ublas::vector<double> Vector;
    typedef boost::numeric::ublas::identity_matrix<double> IdentityMatrix;
    typedef std::deque<double> SampleMemory;

    /** Odwraca macierz (algorytm LU). */
    static bool invertMatrix(const Matrix& input, Matrix& inv);

    /**
        Tworzy instancję obiektu do identyfikacji modeli ARX.

        \param [in] dA Oczekiwany stopień mianownika transmitancji.
        \param [in] dB Oczekiwany stopień licznika transmitancji.
        \param [in] dly Oczekiwane opóźnienie obiektu identyfikowanego.
        \param [in] lambda Współczynnik zapominania (początkowa wartość).
        \param [in] beta Współczynnik wygładzania estymatora szumu.
        \param [in] traceLimit Ograniczenie na ślad macierzy kowariancji.
    */
    explicit RmnkSolver(unsigned rank,
                        double lambda = 0.95,
                        double beta = 0.7,
                        double traceLimit = 100);

    /**
        Zadaje ustawienia domyślne/warunki początkowe.
    */
    void reset();

    /**
        Wykonuje kolejny krok rekurencyjnej metody identyfikacji parametrów modelu.
        Funkcja przyjmuje wejście i wyjście obiektu dyskretnego (lub obiektÓW).
        Wejścia i wyjścia dopisywane są do wektora pamięci próbek i wykorzystywane
        do estymacji nowych parametrów modelu ARX.

        \param [in] inSample Próbka wejściowa identyfikowanego modelu.
        \param [in] outSample Próbka wyjściowa identyfikowanego modelu.
    */
//    void identify(double inSample, double outSample);

    Matrix identify(const Matrix& theta, const Matrix& phi, double outSample);

//    const Polynomial& getEstimatedA() const { return m_estimatedA; }
//    const Polynomial& getEstimatedB() const { return m_estimatedB; }
//    const Polynomial& getEstimatedPhi() const { return m_estimatedPhi; }
//    unsigned getDelay() const { return m_delay; }

private:

    // DEBUG:
    //Polynomial pa, pb;

//    void copytToLocal(const Polynomial& theta, const Polynomial& phi);

    /**
        Zwraca ślad macierzy kwadratowej.
    */
    //double trace(const Matrix& mat);

    Matrix doForgettingMethod(const Matrix& varphi,
                              Matrix &prevCovariance);

    unsigned int m_rankSum;

    double m_lambda;
    double m_beta;
    double m_traceLimit;

    Polynomial m_estimatedA;
    Polynomial m_estimatedB;
    Polynomial m_estimatedPhi;

    SampleMemory m_outmem;          ///< Pamięć dla próbek wyjściowych.
    SampleMemory m_inmem;           ///< Pamięć dla próbek wejściowych.

    Matrix m_covariance;            ///< Macierz kowariancji.
    Matrix m_gainVect;              ///< Wektor wzmocnień (k).

    double m_noiseEstimator;

    // Pomocnicze macierze cache
    Matrix m_covarianceN;
    Matrix m_covarianceD;
};


}
}

#endif // RMNKSOLVER_H
